// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SuggestionDrive.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Runtime.Motion
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Utility functions used for HDP-based obstacle avoidance.
    /// </summary>
    public static class SuggestionDrive
    {
        /// <summary>
        /// Last bin count used to calculate obstacle repelling factor.
        /// </summary>
        private static int lastBinCount; 

        /// <summary>
        /// Last calculated per-bin obstacle repelling factors (dependent on lastBinCount).
        /// </summary>
        private static IEnumerable<double> memoizedObstacleRepellingFactorsByCenterProximity;

        /// <summary>
        /// Trace context.
        /// </summary>
        private static TraceContext traceCtx = TraceContexts.FromString(TraceContexts.Nav, "SuggestionDrive");

        /// <summary>
        /// Adjust drive direction to avoid obstacles.
        /// </summary>
        /// <param name="desiredDriveDirection">Desired drive direction direction.</param>
        /// <param name="hdp">Horizontal depth profile.</param>
        /// <param name="distanceThreshold">Threshold nearer than which force should be 1 for a flat wall.</param>
        /// <param name="headingAdjustmentGain">Gain applied to heading adjustment.</param>
        /// <param name="maximumHeadingAdjustment">Maximum heading adjustment.</param>
        /// <param name="corridorRadiusInMeters">Corridor to left/right of robot within which we consider depth readings.</param>
        /// <returns>Adjusted drive direction, taking obstacles into consideration.</returns>
        public static DriveDirection AdjustDrive(DriveDirection desiredDriveDirection, IHorizontalDepthProfile hdp, double distanceThreshold, double headingAdjustmentGain, double maximumHeadingAdjustment, double corridorRadiusInMeters)
        {
            double heading = desiredDriveDirection.Heading;
            double forward = desiredDriveDirection.Forward;

            if (hdp != null)
            {
                // HDP to simple array of doubles.
                // Future: Fuse IR and sonar into this.
                double[] depths = new double[hdp.Length];
                for (int i = 0; i < hdp.Length; i++)
                {
                    depths[i] = hdp.GetHitPoint(i).X;
                }

                // Adjust drive direction
                if (forward > 0)
                {
                    // Only applied to forward motion. Backing is usually manual or docking.
                    double factor = ForwardThrottlingFactor(depths, distanceThreshold);
                    forward *= factor;
                    double adjustment = HeadingAdjustment(depths, headingAdjustmentGain, maximumHeadingAdjustment);
                    heading += adjustment * forward; // Adjustment proportional to forward speed (freely turn in place next to obstacles)
                    TraceOut.Info(traceCtx, "AdjustDrive: factor: {0}, heading: {1}", factor, adjustment);
                }
            }

            return new DriveDirection(heading, forward, desiredDriveDirection.Distance);
        }

        /// <summary>
        /// Determine forward speed throttling factor as a function of distance readings.
        /// </summary>
        /// <remarks>
        /// Near obstacles, closer to the center have greatest effect; exponentially falling off
        /// with distance and proximity to the center.
        /// </remarks>
        /// <param name="distances">HDP distance readings.</param>
        /// <param name="threshold">Threshold nearer than which force should be 1 for a flat wall.</param>
        /// <returns>Repelling factor (0 - 1).</returns>
        internal static double ForwardThrottlingFactor(IEnumerable<double> distances, double threshold)
        {
            return 1 - Math.Min(1, CalculateRepellingFactors((d, c) => d * Math.Abs(c), distances, threshold).Average());
        }

        /// <summary>
        /// Determine heading adjustment as a function of distance readings.
        /// </summary>
        /// <remarks>Obstacles to the left repel to the right and visa versa.</remarks>
        /// <param name="distances">HDP distance readings.</param>
        /// <param name="gain">Gain applied to adjustment.</param>
        /// <param name="max">Maximum adjustment.</param>
        /// <returns>Adjustment value.</returns>
        internal static double HeadingAdjustment(IEnumerable<double> distances, double gain, double max)
        {
            return Math.Max(-max, Math.Min(max, CalculateRepellingFactors((d, c) => d * c * gain, distances, 0).Sum()));
        }

        /// <summary>
        /// Determine amount of "repelling force" as a function of distance.
        /// </summary>
        /// <remarks>Inversely proportional to square of distance beyond given threshold.</remarks>
        /// <param name="distances">HDP distance readings.</param>
        /// <param name="threshold">Threshold nearer than which force should be 1.</param>
        /// <returns>Repelling factor (0 - 1).</returns>
        internal static IEnumerable<double> ObstacleRepellingFactorByDistance(IEnumerable<double> distances, double threshold)
        {
            return from d in distances
                   let t = Math.Max(double.Epsilon, d - threshold + 1)
                   select Math.Min(1, 1 / (t * t));
        }

        /// <summary>
        /// Determine amount of "repelling force" as a function of proximity to center of view.
        /// </summary>
        /// <remarks>Normalized so that effect sums to 1.</remarks>
        /// <param name="binCount">Number of HDP bins.</param>
        /// <returns>Repelling factor (0 - 1).</returns>
        internal static IEnumerable<double> ObstacleRepellingFactorByCenterProximity(int binCount)
        {
            if (binCount != lastBinCount)
            {
            IEnumerable<double> proximities =
                from b in Enumerable.Range(0, binCount)
                let p = 2.0 / (binCount - 1) * b - 1.0 // -1 .. +1
                select (1 - p * p) * Math.Sign(p);
            double normalize = binCount / proximities.Select(Math.Abs).Sum();
                memoizedObstacleRepellingFactorsByCenterProximity = (from p in proximities select p * normalize).ToArray();
                lastBinCount = binCount;
            }

            return memoizedObstacleRepellingFactorsByCenterProximity;
        }

        /// <summary>
        /// Calculate repelling factors per distance reading as a function of distance and center proximity.
        /// </summary>
        /// <param name="fn">Function applied to pairs of distance and center proximity repelling factors.</param>
        /// <param name="distances">HDP distance readings.</param>
        /// <param name="threshold">Threshold nearer than which force should be 1.</param>
        /// <returns>Repelling factors.</returns>
        private static IEnumerable<double> CalculateRepellingFactors(Func<double, double, double> fn, IEnumerable<double> distances, double threshold)
        {
            return ObstacleRepellingFactorByDistance(distances, threshold)
                       .Zip(ObstacleRepellingFactorByCenterProximity(distances.Count()), fn);
        }
    }
}
